package ch.ethz.fcl.process.polygon;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.Set;

import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;

import ch.ethz.fcl.Constant;
import ch.ethz.fcl.KinectContainer;
import ch.ethz.fcl.input.Coordinate;
import ch.ethz.fcl.tools.Tools;

public class Polygon {
	double prinAxis; // principle axis in degree
	float length;
	int height;
	Point center;

	List<Line> originLines = null;
	List<Point> originPoints = null;

	// rectangle's line map to line in the processed rectangle
	private Map<Line, Line> lineMap;

	private Polygon conductedPolygon;
	private Polygon shrinkedConductedPolygon;

	private Map<Integer, List<Point>> heightPointMap;

	public Polygon() {
		originLines = new ArrayList<Line>();
	}

	public void addPolyLine(Line line) {
		originLines.add(line);

		float angle = line.angleDegree();
		float length = line.length();

		prinAxis = Tools.mergeDegree(prinAxis, this.length, angle, length);
		this.length += length;
	}

	public void setPrinAxis(double angle) {
		this.prinAxis = angle;
	}

	public void setHeight(int height) {
		this.height = height;
	}

	public void addPoints(Point p) {
		if (originPoints == null)
			this.originPoints = new ArrayList<Point>();
		originPoints.add(p);
	}

	// return the minimum point distance between the rectangle and another line
	public double distance(Line line2) {
		double minDist = Double.MAX_VALUE;
		for (Line line : originLines)
			if (!line2.equals(line))
				minDist = Math.min(minDist, line2.distance(line));
		return minDist;
	}

	private void generatePoints() {
		originPoints = new ArrayList<Point>();
		for (Line line : originLines) {
			originPoints.add(line.p1);
		}
	}

	/**
	 * put poly-lines that should be one line into one line and put them into
	 * line map
	 * 
	 * @return
	 */
	private void mapLines() {
		Polygon processedRec = new Polygon();
		lineMap = new HashMap<Line, Line>();

		for (int i = 0; i < originLines.size(); i++) {
			Line line = originLines.get(i);

			// if distance is too large, ignore this line
			if (this.distance(line) > Constant.MIN_LINE_LENGTH * 4)
				continue;

			int sameLineIndex = -1;
			Line sameLine = null;
			for (int j = 0; j < processedRec.originLines.size(); j++) {
				Line line2 = processedRec.originLines.get(j);

				sameLine = line.sameLine(line2);
				if (sameLine != null) {
					sameLineIndex = j;
					break;
				}
			}
			if (sameLine != null) {
				if (Constant.DEBUG) {
					System.out.println("line " + line.angleDegree + " is same "
							+ sameLine.angleDegree);
					System.out.println("line index " + i + " same line index "
							+ sameLineIndex);
				}
				lineMap.remove(processedRec.originLines.get(sameLineIndex));
				lineMap.put(processedRec.originLines.get(sameLineIndex),
						sameLine);
				lineMap.put(line, sameLine);

				processedRec.originLines.remove(sameLineIndex);
				processedRec.addPolyLine(sameLine);
			} else {
				processedRec.addPolyLine(line);
				lineMap.put(line, line);
			}
		}
	}

	/**
	 * 
	 * @param originLines
	 *            original lines that the shape has, to order the lines in the
	 *            list
	 * @return
	 */
	public Polygon generateRectangle(int fixedHeight,
			KinectContainer container, List<Line> originLines) {
		conductedPolygon = new Polygon();

		mapLines();

		int startIndex = 0;
		Line line = originLines.get(startIndex);
		while (lineMap.get(line) == null) {
			startIndex = (startIndex + 1) % originLines.size();
			line = originLines.get(startIndex);

			if (startIndex == 0)
				return null;
		}

		// start from start index such that we can take a round even first lines
		// are null
		for (int i = startIndex; i < originLines.size() + startIndex; i++) {
			line = originLines.get(i % originLines.size());

			if (Constant.DEBUG)
				System.out.println(" line " + i + " angle " + line.angleDegree
						+ " ");

			if (lineMap.get(line) == null)
				continue;

			if (Constant.DEBUG)
				System.out.println(" line " + i % originLines.size()
						+ " in map ");

			int next = 1;
			Line nextLine = originLines.get((i + next) % originLines.size());
			while (lineMap.get(nextLine) == null) {
				next++;
				nextLine = originLines.get((i + next) % originLines.size());
			}

			if (lineMap.get(line).equals(lineMap.get(nextLine))) {
				if (Constant.DEBUG)
					System.out.println("line " + i + " and line " + (i + next)
							+ " is in the same line");

				continue;
			}

			// if two lines are parallel and there are inter lines between these
			// two lines that are not included, probably there should be another
			// line in between
			if (line.isParallel(nextLine) && next != 1) {
				if (Constant.DEBUG)
					System.out.println("line " + i + " and line " + (i + next)
							+ " is parallel");

				int start = (i + 1) % originLines.size();
				int end = (i + next) % originLines.size();

				List<Line> interLines = new ArrayList<Line>();
				if (start > end) {
					interLines.addAll(originLines.subList(start,
							originLines.size()));
					interLines.addAll(originLines.subList(0, end));
				} else
					interLines.addAll(originLines.subList(start, end));

				LineGroup newLineGroup = new LineGroup();
				for (Line l : interLines) {
					newLineGroup.addLine(l);
				}

				// if inter lines length is too big, probably there is another
				// line, so we add only the two neighboring lines into new line
				// group
				if (newLineGroup.length > line.distance(nextLine) * 5) {
					newLineGroup = new LineGroup();
					newLineGroup.addLine(interLines.get(0));
					newLineGroup.addLine(interLines.get(interLines.size() - 1));

					// if the new line is too big, probably should not be a new
					// line added
					if (newLineGroup.length > line.distance(nextLine) * 3)
						continue;
				}

				double lineAngle;
				if (line.isParallel(prinAxis)) {
					lineAngle = (90 + prinAxis) % 180;
				} else {
					lineAngle = prinAxis;
				}

				nextLine = newLineGroup.toLine(lineAngle);

				for (Line interLine : interLines) {
					// change the inter-line angle as it may affect its relation
					// with principle angle
					interLine.angleDegree = lineAngle;
					lineMap.put(interLine, nextLine);
				}

				if (Constant.DEBUG)
					System.out.println("line " + i + " and " + (i + next)
							+ " is paralle and new perpendicular line created");
			}

			if (Constant.DEBUG) {
				System.out.println("line " + i + " angle degree "
						+ line.getLineWithAngle(prinAxis).angleDegree()
						+ " princ axis " + prinAxis);
				System.out.println("next line " + (i + next) + " angle degree "
						+ nextLine.getLineWithAngle(prinAxis).angleDegree());
			}
			Point intersect = line.getLineWithAngle(prinAxis).intersect(
					nextLine.getLineWithAngle(prinAxis));
			if (intersect != null) {
				if (Constant.DEBUG)
					System.out.println("line " + i + " and " + (i + next)
							+ " intersect");
				conductedPolygon.addPoints(intersect);
			}
		}
		if (conductedPolygon.originPoints == null
				|| conductedPolygon.getPoints().size() <= 2)
			return null;

		if (Constant.DEBUG)
			System.out.println("conducted polygon point size "
					+ conductedPolygon.originPoints.size());

		generateShrinkedPolygon(fixedHeight, container,
				conductedPolygon.originPoints);
		return conductedPolygon;
	}

	public List<Polygon> getSplitConductedPolygon() {
		List<Polygon> splitPolygons = new ArrayList<Polygon>();
		for (Integer h : heightPointMap.keySet()) {
			Polygon polygon = new Polygon();
			List<Point> pts = heightPointMap.get(h);
			if (pts == null)
				continue;
			for (Point p : pts)
				polygon.addPoints(p);
			polygon.setHeight(h);
			splitPolygons.add(polygon);
		}

		return splitPolygons;
	}

	/**
	 * for each vertex, create a box surrounding it and return for the points
	 * that lay inside this polygon
	 * 
	 * @return
	 */
	private void generateShrinkedPolygon(int fixedHeight,
			KinectContainer container, List<Point> polygonPoints) {
		shrinkedConductedPolygon = new Polygon();

		heightPointMap = new HashMap<Integer, List<Point>>();

		short[][] heightMap = container.getTableDepthMap().getHeightMap();

		double diagonalAxis1 = (prinAxis + 45) % 180;
		double diagonalAxis2 = (prinAxis + 180 - 45) % 180;

		int lineLength = 16;
		for (int i = 0; i < polygonPoints.size(); i++) {
			boolean shrinkedPolygonAdded = false;
			Point p = polygonPoints.get(i);
			Line line1 = new Line(p, diagonalAxis1, lineLength);
			Line line2 = new Line(p, diagonalAxis2, lineLength);

			if (Coordinate.pointValid(line1.p1)
					&& conductedPolygon.contains(line1.p1)) {
				if (!shrinkedPolygonAdded) {
					shrinkedConductedPolygon.addPoints(line1.p1);
					shrinkedPolygonAdded = true;
				}

				short height = heightMap[(int) line1.p1.y][(int) line1.p1.x];

				height = adjustHeight((int) line1.p1.x, height);

				addHeightPointMap(heightPointMap, height, polygonPoints.get(i));
			}
			if (Coordinate.pointValid(line1.p2)
					&& conductedPolygon.contains(line1.p2)) {
				if (!shrinkedPolygonAdded) {
					shrinkedConductedPolygon.addPoints(line1.p2);
					shrinkedPolygonAdded = true;
				}

				short height = heightMap[(int) line1.p2.y][(int) line1.p2.x];

				height = adjustHeight((int) line1.p2.x, height);

				addHeightPointMap(heightPointMap, height, polygonPoints.get(i));
			}

			if (Coordinate.pointValid(line2.p1)
					&& conductedPolygon.contains(line2.p1)) {
				if (!shrinkedPolygonAdded) {
					shrinkedConductedPolygon.addPoints(line2.p1);
					shrinkedPolygonAdded = true;
				}
				short height = heightMap[(int) line2.p1.y][(int) line2.p1.x];

				height = adjustHeight((int) line2.p1.x, height);

				addHeightPointMap(heightPointMap, height, polygonPoints.get(i));
			}

			if (Coordinate.pointValid(line2.p2)
					&& conductedPolygon.contains(line2.p2)) {
				if (!shrinkedPolygonAdded) {
					shrinkedConductedPolygon.addPoints(line2.p2);
					shrinkedPolygonAdded = true;
				}

				short height = heightMap[(int) line2.p2.y][(int) line2.p2.x];

				height = adjustHeight((int) line2.p2.x, height);

				addHeightPointMap(heightPointMap, height, polygonPoints.get(i));
			}
		}

		if (Constant.DEBUG) {
			System.out.print("got " + heightPointMap.size()
					+ " heights, they are: ");
			for (Integer hE : heightPointMap.keySet())
				System.out.print(" " + hE + " ");
			System.out.println();
		}
		if (heightPointMap.size() == 0)
			return;

		List<Point> zeroHeightPoints = new ArrayList<Point>();
		for (Integer h : heightPointMap.keySet())
			if (h < fixedHeight - Constant.HEIGHT_VARIANCE
					|| h > fixedHeight + Constant.HEIGHT_VARIANCE)
				zeroHeightPoints.addAll(heightPointMap.get(h));

		Iterator<Integer> iterators = heightPointMap.keySet().iterator();
		this.height = iterators.next();
		while ((height < fixedHeight - Constant.HEIGHT_VARIANCE || height > fixedHeight
				+ Constant.HEIGHT_VARIANCE)
				&& iterators.hasNext()) {
			this.height = iterators.next();
		}

		List<Point> points = heightPointMap.get(this.height);
		points.addAll(zeroHeightPoints);
		heightPointMap.put(this.height, points);
	}

	/**
	 * table depth is not flat
	 * 
	 * @param x
	 * @param height
	 * @return
	 */
	private short adjustHeight(int x, short height) {
		if (x < Coordinate.TABLE_MIN_X + 40)
			height += 0;
		else if (x > Coordinate.TABLE_MAX_X - 40)
			height -= 0;

		return height;
	}

	private void addHeightPointMap(Map<Integer, List<Point>> heightPointMap,
			int height, Point p) {
		Integer closetHeight = closestHeight(heightPointMap.keySet(), height);
		if (closetHeight == null) {
			List<Point> pts = new ArrayList<Point>();
			pts.add(p);
			heightPointMap.put(height, pts);
		} else {
			List<Point> pts = heightPointMap.get(closetHeight);
			if (pts.contains(p))
				return;
			else {
				int newHeight = (int) ((closetHeight * pts.size() * 1.0f + height) / (pts
						.size() + 1));
				pts.add(p);
				heightPointMap.remove(closetHeight);
				heightPointMap.put(newHeight, pts);
			}
		}
	}

	private Integer closestHeight(Set<Integer> heights, int height) {
		for (Integer h : heights)
			if (Math.abs(h - height) < Constant.MIN_HEIGHT_DIFF)
				return h;
		return null;
	}

	public List<Point> getPoints() {
		if (originPoints == null)
			generatePoints();
		return originPoints;
	}

	public Polygon getShrinkedPolygon() {
		if (shrinkedConductedPolygon == null)
			return this;
		return this.shrinkedConductedPolygon;
	}

	public Polygon getConductedPolygon() {
		if (conductedPolygon == null)
			return this;
		return this.conductedPolygon;
	}

	/**
	 * check if the polygon contains a point
	 */
	public boolean contains(double x, double y) {
		if (originPoints == null)
			generatePoints();
		int crossings = 0;
		for (int i = 0; i < originPoints.size() - 1; i++) {
			double slope = (originPoints.get(i + 1).x - originPoints.get(i).x)
					/ (originPoints.get(i + 1).y - originPoints.get(i).y);
			boolean cond1 = (originPoints.get(i).y <= y)
					&& (y < originPoints.get(i + 1).y);
			boolean cond2 = (originPoints.get(i + 1).y <= y)
					&& (y < originPoints.get(i).y);
			boolean cond3 = x < slope * (y - originPoints.get(i).y)
					+ originPoints.get(i).x;
			if ((cond1 || cond2) && cond3)
				crossings++;
		}

		double slope = (originPoints.get(0).x - originPoints.get(originPoints
				.size() - 1).x)
				/ (originPoints.get(0).y - originPoints
						.get(originPoints.size() - 1).y);
		boolean cond1 = (originPoints.get(originPoints.size() - 1).y <= y)
				&& (y < originPoints.get(0).y);
		boolean cond2 = (originPoints.get(0).y <= y)
				&& (y < originPoints.get(originPoints.size() - 1).y);
		boolean cond3 = x < slope
				* (y - originPoints.get(originPoints.size() - 1).y)
				+ originPoints.get(originPoints.size() - 1).x;
		if ((cond1 || cond2) && cond3)
			crossings++;

		return (crossings % 2 != 0);
	}

	public boolean contains(Point point) {
		return contains(point.x, point.y);
	}

	public MatOfPoint toMatOfPoint() {
		List<Point> points = getPoints();

		if (points.size() == 0)
			return null;
		Point[] ptsArray = new Point[points.size()];
		points.toArray(ptsArray);
		MatOfPoint matOfPoint = new MatOfPoint(ptsArray);

		return matOfPoint;
	}

	public int getHeight() {
		return height;
	}

	public Point getCenter() {
		if (center == null) {
			List<Point> points = getShrinkedPolygon().getPoints();
			if (points == null)
				return null;
			float x = 0, y = 0;
			for (Point p : points) {
				x += p.x;
				y += p.y;
			}
			x /= points.size();
			y /= points.size();

			center = new Point(x, y);
		}
		return center;
	}
}
